/**
* @brief 求取姿态角
* @param none
* @return none
*/
void makekam_gacc_to_att(FP32 gravity_acc[3],FP32 *ret_roll,FP32 *ret_pitch)
{
    FP32 roll = 0.0f;
    FP32 pitch = 0.0f;
    FP32 flight_z_acc[3] = {0.0f,0.0f,10.0f};
    FP32 flight_x_acc[3] = {10.0f,0.0f,0.0f};
    FP32 plane_xog_normal_vec[3] ={0};
 
    pitch = 90.0f - makekam_vec_ang(flight_x_acc,gravity_acc);/*pitch处于-90°到+90°之间*/
    
    if((90.0f - fabsf(pitch)) > 0.1f)
    {/*pitch与±90度相差大于0.1°*/
        makekam_outer_product(flight_x_acc,gravity_acc,plane_xog_normal_vec);
        roll = makekam_vec_ang(flight_z_acc,plane_xog_normal_vec) - 90.0f;
        if(gravity_acc[2] > 0.0f){
            if(roll < 0.0f){
                roll = -180.0f - roll;
            }else{
                roll = 180 - roll;
            }
        }
    }
    else
    {
        roll = 0.0f;/*判定x轴与重力方向处于一条直线，此为特殊情况，滚转角设定为0°*/
    }
    
    *ret_roll = roll;
    *ret_pitch = pitch;
}

/**
* @brief 通过外积法，求平面两个非同向向量所在平面的法向量
* @param    vec_a   向量1
            vec_b   向量2
* @return 夹角
*/
static void makekam_outer_product(const FP32 vec1[3],const FP32 vec2[3],FP32 vecDst[3])
{
    vecDst[0] = vec1[1]*vec2[2] - vec2[1]*vec1[2];/*y1z2-y2z1*/
    vecDst[1] = vec2[0]*vec1[2] - vec1[0]*vec2[2];/*x2z1-x1z2*/
    vecDst[2] = vec1[0]*vec2[1] - vec2[0]*vec1[1];/*x1y2-x2y1*/
}

/**
* @brief 内积法求向量夹角
* @param    vec_a   向量1
            vec_b   向量2
* @return 夹角
*/
static FP32 makekam_vec_ang(const FP32 vec_a[3],const FP32 vec_b[3])
{
    FP32 angle = 0.0f;
    FP32 mod_of_vec_a = 0.0f;
    FP32 mod_of_vec_b = 0.0f;
    mod_of_vec_a = get_vector_mod(vec_a);
    mod_of_vec_b = get_vector_mod(vec_b);
    if((0.001f >= mod_of_vec_a) || (0.001f >= mod_of_vec_b)){
        angle = 0.0f;/*无法计算某向量与零向量的夹角，返回0*/
    }else{
        FP32 cos_value = (vec_a[0]*vec_b[0] + vec_a[1]*vec_b[1] + vec_a[2]*vec_b[2]) / (mod_of_vec_a * mod_of_vec_b);
        if(cos_value > 1.0f){
            cos_value=1.0f;
        }
        if(cos_value < -1.0f){
            cos_value=-1.0f;
        }
        angle = acos_approx(cos_value);
        angle = angle * 180.0f / 3.1415f;
    }
    return angle;
}

/**
* @brief 计算重力加速度向量模
* @param none
* @return none
*/
static FP32 get_vector_mod(const FP32 vector[3])
{
    return sqrtf(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
}